import Blender, GameLogic
from Blender import Material, Object

from middleware.yarp import YarpBlender

# Retrieve the YARP port we want to use
yarp_port = YarpBlender.getPort('/blender_simu/cog_map')

scene = GameLogic.getCurrentScene()

# get OBCameraRobot from object list.
#We'll use it to check if tracked objects are visible or not.
cam = scene.getObjectList()["OBCameraRobot"]

# GameLogic.trackedObjects is initialized in initialization.py. 
# It holds the list of tracked objects (basically, meshes) + their bounding boxes).
trackedObjects = GameLogic.trackedObjects
visibleObjects = GameLogic.visibleObjects

#Little helper fonction to calculate the hue of a color
def RGBtoHue(rgbList):
	R = rgbList[0]
	G = rgbList[1]
	B = rgbList[2]
	
	# min, max, delta;
	min_rgb = min( R, G, B )
	max_rgb = max( R, G, B )
	delta = max_rgb - min_rgb

	if not delta:
		return 0
	if R == max_rgb:
		H = ( G - B ) / delta # between yellow & magenta
	elif G == max_rgb:
		H = 2 + ( B - R ) / delta # between cyan & yellow
	else:
		H = 4 + ( R - G ) / delta # between magenta & cyan
	H *= 60 # convert to deg
	if H < 0:
		H += 360
	return int(H)

#this method retrieve the first material of the first mesh of an object
#and return the hue of this material.
def retrieveHue(obj):
	mesh = obj.getMesh(0) # There can be more than one mesh...
	if mesh != None:
		bMat = Material.Get(mesh.getMaterialName(0)[2:])
		return RGBtoHue(bMat.getRGBCol())
	
	return None

#the main method: check is an object lies inside of the camera frustrum. 	
def checkVisible(obj):
	# camera inside box?
	bb = trackedObjects[obj] #trackedObjects was filled at initialization with the object's bounding boxes
	pos = obj.getPosition()
	#translate the bounding box to the current object position and check if it is in the frustrum
	if cam.boxInsideFrustum([[bb_corner[i] + pos[i] for i in range(3)] for bb_corner in bb]) != cam.OUTSIDE:
	# object is inside
		return True

for obj in trackedObjects:
	#if the object is visible and no yet in the visibleObjects list...
 	if checkVisible(obj) and visibleObjects.count(obj) == 0:
		visibleObjects.append(obj)
		print obj.getName(), '(', obj.objClass,', hue:',retrieveHue(obj),') just appeared.'

		# prepare the YARP message...
		bottle = yarp_port.prepare()
		bottle.clear()
		bottle.addString(obj.getName() + '(' + obj.objClass + ') just appeared.')
		bottle.addInt(retrieveHue(obj))
		#...and send it
		yarp_port.write()

	#if the object is not visible but was in the visibleObjects list...
 	if not checkVisible(obj) and visibleObjects.count(obj) != 0:
		visibleObjects.remove(obj)
		print obj.getName(), ' just disappeared.'

		# prepare the YARP message...
		bottle = yarp_port.prepare()
		bottle.clear()
		bottle.addString(obj.getName() + '(' + obj.objClass + ') just disappeared.')
		#...and send it
		yarp_port.write()
